/**
 * 
 */
package com.fuzzknuckle.multimixconfig;

/**
 * @author jeff
 *
 */
public class MultiArm {
	
	//units are unimportant but should be consistent.
	public double originX, originY, motorX, motorY;
	
	//define an arm given origin, angle and length.
	//angle is in degrees from straight ahead.
	//x axis is from left to right of multi.
	//y axis is from back to front of multi.
	// x = 0, y=0 is the COG of the multi.
	public MultiArm(double originX, double originY,
				double angle, double length) {
		this.originX = originX;
		this.originY = originY;
		double angleInRadians = angle * Math.PI / 180;
		
		this.motorY = Math.sin(angleInRadians) * length + originY;
		this.motorX = Math.cos(angleInRadians) * length + originX;
	}
	
	public MultiArm() {
		this.originX = 0;
		this.originY = 0;
		this.motorX = 0;
		this.motorY = 0;
	}
	
	public void setOrigin(double originX, double originY) {
		this.originX = originX;
		this.originY = originY;
	}
	
	public void setMotor(double motorX, double motorY) {
		this.motorX = motorX;
		this.motorY = motorY;
	}

	public double getOriginX() {
		return originX;
	}

	public double getOriginY() {
		return originY;
	}

	public double getMotorX() {
		return motorX;
	}

	public double getMotorY() {
		return motorY;
	}

	@Override
	public String toString() {
		return "MultiArm [originX=" + originX + ", originY=" + originY
				+ ", motorX=" + motorX + ", motorY=" + motorY + "]";
	}
	
}
